#pragma once

#include "HapticAPI.h"
#include "FcsHapticMASTER.h"
#include "FcsConstantForce.h"
#include "FcsSphere.h"
#include "FcsBlock.h"
#include "ScopeAPI.h"
#include <vector>
#include <string>
#pragma comment(lib, "HapticAPI.lib")
#pragma comment(lib, "ScopeAPI.lib")

///// <summary>
///// matlab-y stuff
///// </summary>
//#pragma comment(lib, "libdfblas.lib")
//#pragma comment(lib, "libdflapack.lib")
//#pragma comment(lib, "libemlrt.lib")
//#pragma comment(lib, "libeng.lib")
//#pragma comment(lib, "libfixedpoint.lib")
//#pragma comment(lib, "libmat.lib")
//#pragma comment(lib, "libmex.lib")
//#pragma comment(lib, "libmwblas.lib")
//#pragma comment(lib, "libmwlapack.lib")
//#pragma comment(lib, "libmwmathutil.lib")
//#pragma comment(lib, "libmwservices.lib")
//#pragma comment(lib, "libmwsl_solver_rtw.lib")
//#pragma comment(lib, "libmx.lib")
//#pragma comment(lib, "libut.lib")

namespace HapticDevice {

	using namespace std;

//	namespace matlab {
//#include "mat.h" // for matlab-y stuff
//	}

	enum HM_CONSTANTS {
		NAXES = 3,			// primary axes
		NCOORDS = 4,		// primary coordinates
		NSENSORS = 15,		// total number of sensors
		NSENSORST = 16,		// sensors including time
		NCALIBPTS = 100,	// number of calibration points
		MAXFREQ	= 2500
	};

	enum AXIS_LIST {
		X = 0,
		Y = 1,
		Z = 2,
		THETA = 3
	};

	enum SENSOR_LIST {
		T = 0,			// time
		PX, PY, PZ,		// position
		VX, VY, VZ,		// velocity
		AX, AY, AZ,		// acceleration
		FX, FY, FZ,		// force
		GX, GY, GZ		// gimbal angles
	};

	class ChmDevice
	{
	private:
		/// <summary>
		/// hardware settings
		/// </summary>
		wstring					deviceName;		//typically, "vmd"
		wstring					ipAddress;		//ip address of the device
		unsigned				port;			//port number of the device
		CFcsHapticMASTER*		hm;				//pointer to the haptic master object

		/// <summary>
		/// haptic environment settings
		/// </summary>
		FCSSTATE				currentState;	// current state of the haptic master
		double					mass;			// mass to simulate
		double					dampingRatio;	// damping ratio
		double					envK[3][3];		// environment stiffness
		double					envB[3][3];		// environment damping

		CFcsConstantForce*		pForce;			// haptic force object
		CFcsBlock*				pBlock;			// haptic block (for setting up cuboidal limits)
		CFcsSphere*				pSphere;		// haptic sphere (for setting up spherical limits)

	
		/// <summary>
		/// data collection settings
		/// </summary>
		unsigned				startTime;		// count that maintains the startTime
		double					duration;		// duration for data collection
		double					sRate;			// data collection rate
		vector<double>			dataVec;		// data collected for one time instant
		vector<vector<double>>	dataMat;		// data collected for a trial
		FcsInfoBuffer*			dataBuf;		// buffer used for collecting data -- internal operations only
		wstring					fileName;		// filename for data collection

		vector<double>			curFor;			// current force
		vector<double>			curPos;			// current position
		vector<double>			curGim;			// current gimbal

		/// <summary>
		/// device calibration settings
		/// </summary>
		vector<double>			zeroPos;		// position_{zero}
		vector<double>			zeroGim;		// gimbal_{zero}

	public:
		ChmDevice(wstring);
		~ChmDevice(void);

		/// <summary>
		/// hardware functions
		/// </summary>
		bool Connect(wstring);					// connect to the hardware device (i.e., initialize)
		bool Disconnect(void);					// disconnect from the hardware device

		/// <summary>
		/// haptic environment functions
		/// </summary>
		bool SetState(FCSSTATE state);			// set current the hapticMASTER state
		FCSSTATE GetState(void);				// get the current hapticMASTER state
		bool SetMass(double mass);								// set the mass
		void SetEnvironment(double k[3][3], double b[3][3]);			// set the haptic environment 
		void SetEnvironment(double k[3][3], double zeta);		// set the haptic environment
		FcsInfoBuffer* renderForce(void);						// render the appropriate haptic force

		void CreateSphericalLimits(double radius, double center[3]);	// set spherical limits
		void CreateCuboidalLimits(double lengths[3], double center[3]);	// set cuboidal limits
		void CreateConstantForce(double force[3]);			// create a constant force object
		void SetBiasForce(double force[3]);

		/// <summary>
		/// device calibration functions
		/// </summary>
		bool ZeroForce(void);					// zero the forces
		bool ZeroPosition(void);				// zero the position
		bool ZeroGimbal(void);					// zero the gimbal

		/// <summary>
		/// data collection functions
		/// </summary>
		bool ArmCollection(void);				// arm the data collection routine to run
		bool CollectData(void);					// collect the data
		wstring SaveData(wstring fileName);		// save the file


		/// <summary>
		/// error collection -- eventually needs to be replaced by an exception handling class
		/// </summary>
		wstring errStr;
	};

}